/*
 * ColorLaneSensorPoller.cpp
 *
 *  Created on: 11 jan. 2013
 *      Author: Admin
 */

#include "LaneSensorPoller.hpp"
#include "LaneEvent.hpp"
#include "Robot.hpp"

LaneSensorPoller::LaneSensorPoller()	:
	leftColorLaneSensor(new ColorLaneSensor(CENTRALLEFT)),
	rightColorLaneSensor(new ColorLaneSensor(CENTRALRIGHT)),
//	leftSideExitSensor(new ColorLaneSensor(OUTERLEFT)),
//	rightSideExitSensor(new ColorLaneSensor(OUTERRIGHT)),
//	colorLaneStatus(ONCOLORLANE),
//	sideExitStatus(ONCOLORLANE),
	colorLaneSensorStatus(BOTHON)
//	sideExitSensorStatus(BOTHON)
{ }

LaneSensorPoller::~LaneSensorPoller()
{
	delete leftColorLaneSensor;
	delete rightColorLaneSensor;
}

void LaneSensorPoller::pollLaneSensors()
{
	LaneSensorStatus oldColorLaneSensorStatus = colorLaneSensorStatus;
//		, oldSideExitSensorStatus = sideExitSensorStatus;
	updateLaneSensorStatus();

	if (	oldColorLaneSensorStatus == colorLaneSensorStatus)/* &&
			oldSideExitSensorStatus == sideExitSensorStatus)*/
		return;
	//If this point is reached a sensor change has taken place, forward this to the robot
	LaneEvent event = LaneEvent(oldColorLaneSensorStatus, colorLaneSensorStatus);/*,
								oldSideExitSensorStatus, sideExitSensorStatus);*/
	Robot::getInstance().handleEvent(event);
}

void LaneSensorPoller::updateLaneSensorStatus()
{
	bool left = leftColorLaneSensor->poll(), right = rightColorLaneSensor->poll();
	if (left && right) colorLaneSensorStatus = BOTHON;
	else if (left) colorLaneSensorStatus = LEFTON;
	else if (right) colorLaneSensorStatus = RIGHTON;
	else colorLaneSensorStatus = BOTHOFF;
}

//void LaneSensorPoller::updateSideExitStatus()
//{
//	bool left = leftSideExitSensor->poll(), right = rightSideExitSensor->poll();
//	if (left && right) sideExitSensorStatus = BOTHON;
//	else if (left) sideExitSensorStatus = LEFTON;
//	else if (right) sideExitSensorStatus = RIGHTON;
//	else sideExitSensorStatus = BOTHOFF;
//}

LaneSensorStatus LaneSensorPoller::getColorLaneSensorStatus()
{
	return colorLaneSensorStatus;
}

//LaneSensorStatus LaneSensorPoller::getSideExitSensorStatus()
//{
//	return sideExitSensorStatus;
//}

//LaneSensorPoller::LaneSensorChange LaneSensorPoller::getChangeFrom(LaneSensorStatus oldStatus, bool leftSensor, bool rightSensor)
//{
//	switch (oldStatus)
//	{
//	case BOTHON:
//		if (leftSensor && rightSensor) return NOCHANGE;
//		else if (leftSensor) return RIGHTOFF;
//		else if (rightSensor) return LEFTOFF;
//		else return BOTHOFF;
//	case BOTHOFF:
//		if (leftSensor && rightSensor) return BOTHON;
//		else if (leftSensor) return LEFTON;
//		else if (rightSensor) return RIGHTON;
//		else return NOCHANGE;
//	case LEFTON:
//		if (leftSensor && rightSensor) return RIGHTON;
//		else if (leftSensor) return NOCHANGE;
//		else if (rightSensor) return LEFTOFFRIGHTON;
//		else return LEFTOFF;
//	case RIGHTON:
//		if (leftSensor && rightSensor) return LEFTON;
//		else if (leftSensor) return LEFTONRIGHTOFF;
//		else if (rightSensor) return NOCHANGE;
//		else return RIGHTOFF;
//	}
//	return NOCHANGE;
//}
//
//void LaneSensorPoller::handlePoll(LaneSensorChange colorLaneSensorChange, LaneSensorChange sideExitSensorChange)
//{
//	ColorLaneStatus oldColorLaneStatus = colorLaneStatus,
//					oldSideExitStatus = sideExitStatus;
//
//
//
//
//
//	//Handle color lane sensor changes
//	if (colorLaneSensorChange != NOCHANGE)
//	{
//		ColorLaneStatus oldColorLaneStatus = colorLaneStatus;
//			switch (colorLaneStatus)
//			{
//			case ONCOLORLANE:
//				if (colorLaneSensorChange == RIGHTOFF) colorLaneStatus = LEFTSIDELEAVINGCOLORLANE;
//				else if (colorLaneSensorChange == LEFTOFF) colorLaneStatus = RIGHTSIDELEAVINGCOLORLANE;
//				else if (colorLaneSensorChange == BOTHOFF) colorLaneStatus = SENSORSONCOLORLANE;
//				break;
//			case OUTSIDECOLORLANE:
//				if (colorLaneSensorChange == RIGHTOFF) colorLaneStatus = RIGHTSIDEENTERINGCOLORLANE;
//				else if (colorLaneSensorChange == LEFTOFF) colorLaneStatus = LEFTSIDEENTERINGCOLORLANE;
//				else if (colorLaneSensorChange == BOTHOFF) colorLaneStatus = SENSORSONCOLORLANE;
//				break;
//			case LEFTSIDELEAVINGCOLORLANE:
//				if (colorLaneSensorChange == LEFTOFF)
//					colorLaneStatus = SENSORSONCOLORLANE;
//				else if (colorLaneSensorChange == RIGHTON && Robot::getInstance().getEngine().getState() == Engine::LEFT)
//					colorLaneStatus = OUTSIDECOLORLANE;
//				else if (colorLaneSensorChange == RIGHTON)
//					colorLaneStatus = ONCOLORLANE;
//				break;
//			case RIGHTSIDELEAVINGCOLORLANE:
//				if (colorLaneSensorChange == RIGHTOFF)
//					colorLaneStatus = SENSORSONCOLORLANE;
//				else if (colorLaneSensorChange == LEFTON && Robot::getInstance().getEngine().getState() == Engine::RIGHT)
//					colorLaneStatus = OUTSIDECOLORLANE;
//				else if (colorLaneSensorChange == LEFTON)
//					colorLaneStatus = ONCOLORLANE;
//				break;
//			case LEFTSIDEENTERINGCOLORLANE:
//				if (colorLaneSensorChange == LEFTON) colorLaneStatus = ONCOLORLANE;
//				if (colorLaneSensorChange == RIGHTOFF) colorLaneStatus = SENSORSONCOLORLANE;
//				break;
//			case RIGHTSIDEENTERINGCOLORLANE:
//				if (colorLaneSensorChange == RIGHTON) colorLaneStatus = ONCOLORLANE;
//				if (colorLaneSensorChange == LEFTOFF) colorLaneStatus = SENSORSONCOLORLANE;
//				break;
//			case SENSORSONCOLORLANE:
//				if (colorLaneSensorChange == RIGHTON) colorLaneStatus = LEFTSIDELEAVINGCOLORLANE;
//				else if (colorLaneSensorChange == LEFTON) colorLaneStatus = RIGHTSIDELEAVINGCOLORLANE;
//				else if (colorLaneSensorChange == BOTHON) colorLaneStatus = ONCOLORLANE;
//				break;
//			}
//			if (colorLaneStatus == oldColorLaneStatus)
//				return;
//
//	}
//
//	//Handle side-exit sensor changes
//
//
//	ColorLaneEvent event = ColorLaneEvent(oldColorLaneStatus, colorLaneStatus, oldSideExitStatus, );
//	Robot::getInstance().handleEvent(event);
//}
//
//ColorLaneStatus LaneSensorPoller::getColorLaneStatus()
//{
//	return colorLaneStatus;
//}
